
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import javax.bluetooth.RemoteDevice;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Delay;

public class eva_mae {
	static DifferentialPilot pilot = new DifferentialPilot(4f, 16.1f, Motor.A, Motor.C); 
	 static UltrasonicSensor Sonic = new UltrasonicSensor(SensorPort.S4);
	 static int distSen;
	
		
	 public static void main(String[] args) throws Exception {
	
		eva_mae se= new eva_mae();
				
		
		String name = "NXT";
	    LCD.drawString("Conectando...", 0, 0);
	    RemoteDevice btrd = Bluetooth.getKnownDevice(name);
        
	    if (btrd == null) {
	      LCD.clear();
	      LCD.drawString("No encontro NXT", 0, 0);
	      Button.waitForAnyPress();
	      System.exit(1);
	    }

	    BTConnection btc = Bluetooth.connect(btrd);

	    if (btc == null) {
	      LCD.clear();
	      LCD.drawString("Conexion fallo", 0, 0);
	      Button.waitForAnyPress();
	      System.exit(1);
	    }
	  
	    LCD.clear();
	    LCD.drawString("Conectado", 0, 0);

	    DataInputStream dis = btc.openDataInputStream();
	    DataOutputStream dos = btc.openDataOutputStream();
	    
	    while(!Button.ESCAPE.isDown() ){
	    pilot.forward();
	    LCD.drawString("Avanzando...",0,4);
	    
		distSen=Sonic.getDistance();
	 	LCD.drawInt(distSen,4, 0, 7);
	 	
	 	if (distSen < 30){ 
	 	se.gir(); 
	 	   
	     boolean enviar= true;
	    	try{
				dos.writeBoolean(enviar);
				dos.flush();
			} catch (IOException ioe) {
		          LCD.drawString("Write Exception", 0, 0);
		       }
	    	
	    	se.gir();
	}
	    try {
	        LCD.drawChar(dis.readChar(),0,3);
	      } catch (IOException ioe) {
	        LCD.drawString("Read Exception ", 0, 0);
	      }
		try {
		      LCD.drawString("Closing... ", 0, 0);
		      dis.close();
		      dos.close();
		      btc.close();
		    } catch (IOException ioe) {
		      LCD.drawString("Close Exception", 0, 0);
		    }
		  
		    LCD.drawString("Finished",3, 4);
		    Button.waitForAnyPress();

}
}
	public void gir()
	{
		
		   LCD.clear();
		   LCD.drawString("'evacion'", 0, 7);
		   pilot.stop();
		   pilot.travel(-15);
		   pilot.rotate(45);

		  Delay.msDelay(20000);
		  
	}
}
